Charting Courses in 2D and 3D:
Motion Planning in Robotics

Keren Garcia, Wolfram Research

#WolframTechConf

Outline

Motion Planning

Concepts

Methods

Applications

What is Motion Planning?

Process of finding an efficient path for a robot to move from one point to another, while avoiding obstacles.

Motion Planning Problem

How can a robot move from point A to point B without colluding with obstacles?

Valid Path 	          		Invalid Path

Concepts - Robot

What kind of robot is being used and for what?

Concepts - Workspace

Physical space where the robot and obstacles exist (2D or 3D environment)

Concepts - Configuration Space

The space where the robot can move around after expanding the obstacles in the workspace by the robot’s dimensions.

Challenges in Motion Planning

High dimensionality

Hard to compute paths

Real-time constraints for dynamic environments

Optimality vs. computational speed

Motion Planning Algorithms

Graph-Based Methods

Sampling-Based Methods

Optimization-Based Methods

Modern Methods

Graph-Based Algorithms

Graph search algorithms to compute discrete paths in the robot’s state space.

Simple

Computationally heavy

Sampling-Based Algorithms

Configuration space is randomly sampled and for each sampling point, it is checked whether the robot’s configuration is collision-free.

Works well in high-dimensional spaces

Paths not optimal

Optimization-Based Algorithms

({{}})

Start with an initial path and optimize it to minimize cost functions while avoiding collisions.

Smooth paths

Computationally complex

Modern/Hybrid Algorithms

Combine sampling, optimization and machine learning algorithms.

Optimal solutions

High computational times

How Motion Planning Works

1. Model robot and workspace
2. Build the configuration space
3. Choose algorithm
4. Check for possible collisions
5. Refine path
6. Execute

Applications

Autonomous vehicles

Industrial robots

Drones

Humanoid robots

Applications - Mobile Robot

A mobile robot is provided information through a floor plan, in this case, a living room.

Workflow - Mobile Robot

{robot,furniture} ={ ,};
cuboids=BoundingRegion[#,"MinOrientedCuboid"]& /@ ConnectedMeshComponents[furniture];
workspace=RegionUnion[ConvexHullMesh[Part[PolyhedronCoordinates[CanonicalizePolyhedron[#]],All,1;;2]]& /@ cuboids];
r=Last[ BoundingRegion[robot,"MinBall"]];
{a}={Most[RegionCentroid[robot]]};
Framed[Graphics[{Hue[0.6,0.3,0.95], workspace, Red,Disk[{Most[RegionCentroid[robot]]}, r],}]]

The floor plan can be translated to a 2D workspace.

Workflow - Mobile Robot

{a}={Most[RegionCentroid[robot]]};
Framed[Graphics[{Hue[0.6,0.3,0.95],EdgeForm[{Hue[0.6,0.3,0.95],Dashed}],configuration=BoundaryMeshRegion[RegionDilation[workspace,r],MeshCellStyle->{2->HatchFilling[],1->Dashed}], workspace, Red,PointSize[.01],Point[{a}], Black,Text["Reference Point",Offset[a, {7,-9.8}], Automatic]}]]

Simplify the task by representing the robot as a reference point and expanding the obstacles by the robot’s dimensions.

The enlarged furniture is now the configuration space.

Workflow - Mobile Robot

FindShortestCurve can be used to find an efficient path through the configuration space.

boundary=RegionResize[BoundingRegion[configuration],Scaled[1.5]];
optspace=RegionDifference[boundary, configuration];
{a,b}={Most[RegionCentroid[robot]],{20,18}};

path=FindShortestCurve[optspace,a,b];

Framed[Graphics[{Hue[0.6,0.3,0.95],EdgeForm[{Hue[0.6,0.3,0.95],Dashed}],configuration,workspace,Red,path,PointSize[.01],Point[{a,b}]}]]

Workflow - Mobile Robot

The computed path can be translated into a plan for the robot, determining distances for the robot to travel and angles to turn.

plan={Pi-ResourceFunction["SignedVectorAngle"][#[[3]]-#[[2]],#[[1]]-#[[2]]],EuclideanDistance @@ Rest[#]} & /@Partition[Prepend[First[path],{0.,a[[2]]}],3,1];
Dataset[AssociationThread[{"Angle","Distance"}->#]&/@Chop[plan],MaxItems->6]

Workflow - Mobile Robot

Visualizing the robot following the plan, it will move along the calculated path, adjusting its course and avoiding collisions with the furniture.

transformations=AnglePath3D[RegionCentroid[robot],{Last[#],{First[#],0}} & /@ plan,"RotationTranslation"];
transformations=transformations[[Join[Range[1,Length[transformations],9],{}]]]; 
robot2=Translate[MeshPrimitives[robot,1],-RegionCentroid[robot]];
Show[furniture,Graphics3D[{Red,Dashed,Line[ArrayPad[path[[1]],{0,{0,1}}]],Opacity[0.2],Hue[0.6,0.3,0.95],GeometricTransformation[robot2, transformations],Thickness[.01]}],Boxed->True,SphericalRegion->False, ImageSize->200]

Summary

Motion Planning involves find a collision-free path for a robot from point A to point B

Central problem in autonomous robotics (navigation, manipulation, drones, production lines, etc.)

Workspace versus the configuration space

Various types of algorithms to approach motion planning

Balancing optimality (shortest, smoothest) vs. computational efficiency

From Puzzles to Graphs: Coloring Your Way through Sudoku

Alejandra Ortiz Duran, Friday 8:30 AM CDT

Wolfram R&D YouTube Channel
@WolframRD

#WolframTechConf